#include <ros/ros.h>
#include <iostream>
#include <unistd.h>
#include <string>
#include <std_msgs/Empty.h>
#include <std_msgs/String.h> 
#include <std_msgs/Header.h>
#include <std_msgs/Float64.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/VFR_HUD.h>
#include <mavros_msgs/Altitude.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <sensor_msgs/NavSatFix.h>
#include "outdoor_offb/Uav_state.h"
#include "outdoor_offb/Cmd_send.h"
#define STATE_UAV 101
#define CMD_UAV 102
#define SELF_CMD 103
#define ARM 1
#define TAKEOFF 2
#define LAND 3
#define POS 4
#define VEL 5
#define ACC 6
#define POS_VEL 7
#define POS_ACC 8
#define VEL_ACC 9
#define POS_VEL_ACC 10
#define OPEN_TRA 11

// uint8_t uav_num=1;
std::string uav_id = "plane_1";

outdoor_offb::Cmd_send recv_cmd;
outdoor_offb::Cmd_send self_cmd;
std_msgs::Float64 uav_rel_alt;
std_msgs::Float64 uav_compass_hdg;
mavros_msgs::State current_state;
mavros_msgs::VFR_HUD air_ground_speed;
mavros_msgs::Altitude altitude_from_px4;
sensor_msgs::NavSatFix uav_global_pos;
geometry_msgs::TwistStamped uav_global_vel;
geometry_msgs::PoseStamped uav_local_pos;
geometry_msgs::TwistStamped uav_local_vel;


void recv_cmd_cb(const outdoor_offb::Cmd_send::ConstPtr &msg){
    recv_cmd=*msg;
}

void self_cmd_cb(const outdoor_offb::Cmd_send::ConstPtr &msg){
    self_cmd=*msg;
}

void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

void global_pos_cb(const sensor_msgs::NavSatFix::ConstPtr& msg){
    uav_global_pos = *msg;
}

void global_vel_cb(const geometry_msgs::TwistStamped::ConstPtr& msg){
    uav_global_vel = *msg;
}

void rel_alt_cb(const std_msgs::Float64::ConstPtr& msg){
    uav_rel_alt = *msg;
}

void compass_hdg_cb(const std_msgs::Float64::ConstPtr& msg){
    uav_compass_hdg = *msg;
}

void local_pos_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
    uav_local_pos = *msg;
}

void local_vel_cb(const geometry_msgs::TwistStamped::ConstPtr& msg){
    uav_local_vel = *msg;
}

void air_ground_speed_cb(const mavros_msgs::VFR_HUD::ConstPtr& msg){
	air_ground_speed = *msg;
}

void altitutude_cb(const mavros_msgs::Altitude::ConstPtr &msg){
	altitude_from_px4 = *msg;
}

double Limit(const double &num, const double &Max){
    return num>Max? Max:(num<-Max? -Max:num);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;

	// ###########################订阅mavros消息##########################################
	// 
    //读取PX4的实时状态
    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            (uav_id + "/mavros/state", 100, state_cb);
    //读取PX4的全局位置，速度（融合后的速度），相对高度，磁罗盘
    ros::Subscriber global_pos_sub = nh.subscribe<sensor_msgs::NavSatFix>
            (uav_id + "/mavros/global_position/global", 100, global_pos_cb);
    ros::Subscriber global_vel_sub = nh.subscribe<geometry_msgs::TwistStamped>
            (uav_id + "/mavros/global_position/gp_vel", 100, global_vel_cb);
    ros::Subscriber rel_alt_sub = nh.subscribe<std_msgs::Float64>
            (uav_id + "/mavros/global_position/rel_alt", 100, rel_alt_cb);
    ros::Subscriber compass_hdg_sub = nh.subscribe<std_msgs::Float64>
            (uav_id + "/mavros/global_position/compass_hdg", 100, compass_hdg_cb);
    //读取PX4的局部位置，速度（可能只是FCU的速度）
    ros::Subscriber local_pos_sub = nh.subscribe<geometry_msgs::PoseStamped>
            (uav_id + "/mavros/local_position/pose", 100, local_pos_cb);
    ros::Subscriber local_vel_sub = nh.subscribe<geometry_msgs::TwistStamped>
            (uav_id + "/mavros/local_position/velocity_local", 100, local_vel_cb);
	//读取PX4的空速和地速
	ros::Subscriber air_ground_speed_sub = nh.subscribe<mavros_msgs::VFR_HUD>
			(uav_id + "/mavros/vfr_hud", 100, air_ground_speed_cb);
	//订阅高度
	ros::Subscriber altitude_sub = nh.subscribe<mavros_msgs::Altitude>
			(uav_id + "/mavros/altitude", 100, altitutude_cb);
	// 
	// ###########################订阅mavros消息##########################################
    // 
	// 
	// ###########################服务###################################################
	// 
	//和PX4相关的解锁，切换模式，起飞，降落服务
    // ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
    //         (uav_id + "/mavros/cmd/arming");
    // ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
    //         (uav_id + "/mavros/set_mode");
    // ros::ServiceClient takeoff_client = nh.serviceClient<mavros_msgs::CommandTOL>
    //         (uav_id + "/mavros/cmd/takeoff");
    // ros::ServiceClient land_client = nh.serviceClient<mavros_msgs::CommandTOL>
    //         (uav_id + "/mavros/cmd/land");
	// 
	// ###########################服务###################################################
    // 
	// 
	// ###########################发布的消息##############################################
	// 
	//向PX4发布位置，速度，加速度，偏航指令
    // ros::Publisher local_pos_pub = nh.advertise<mavros_msgs::PositionTarget>
    //         (uav_id + "/mavros/setpoint_raw/local",100);
    //发布无人机的状态（集成的状态）
    ros::Publisher uav_state_pub = nh.advertise<outdoor_offb::Uav_state>
            ("/outdoor/uav/state",1);
	// 
	// ###########################发布的消息##############################################

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20);

    // mavros_msgs::SetMode offb_set_mode;
    // offb_set_mode.request.custom_mode = "OFFBOARD";

    // mavros_msgs::CommandBool arm_cmd;
    // arm_cmd.request.value = true;

    // mavros_msgs::CommandTOL takeoff_cmd;
    // takeoff_cmd.request.altitude = 1;

    // mavros_msgs::CommandTOL land_cmd;
    // land_cmd.request.altitude = 0;

    // mavros_msgs::PositionTarget pos_tag;
    // pos_tag.header.frame_id="UAV";
    // pos_tag.coordinate_frame=mavros_msgs::PositionTarget::FRAME_LOCAL_NED;
    // pos_tag.yaw=0;

    outdoor_offb::Uav_state uav_state;
    uav_state.frame_id=STATE_UAV;
    uav_state.uav_index=1;

	ROS_INFO("Sending /plane_1/mavros/states to serial port");
    while(ros::ok()){
		ROS_INFO("Send UAV state to /outdoor/uav/state !");
        uav_state.connected = current_state.connected;
        uav_state.armed = current_state.armed;
        if (current_state.mode=="MANUAL"){uav_state.mode = 0;}
        else if (current_state.mode=="STABILIZED"){uav_state.mode = 1;}
        else if (current_state.mode=="POSCTL"){uav_state.mode = 2;}
        else if (current_state.mode=="OFFBOARD"){uav_state.mode = 3;}
        else if (current_state.mode=="ALTCTL"){uav_state.mode = 4;}
        else {uav_state.mode = 255;}
        uav_state.latitude = uav_global_pos.latitude;
        uav_state.longitude = uav_global_pos.longitude;
        uav_state.altitude = uav_global_pos.altitude;
        uav_state.global_lin_x = uav_global_vel.twist.linear.y;
        uav_state.global_lin_y = uav_global_vel.twist.linear.x;
        uav_state.global_lin_z = uav_global_vel.twist.linear.z;
        uav_state.global_ang_x = uav_global_vel.twist.angular.x;
        uav_state.global_ang_y = uav_global_vel.twist.angular.y;
        uav_state.global_ang_z = uav_global_vel.twist.angular.z;
        uav_state.rel_altitude = uav_rel_alt.data;
        uav_state.deg_compass = uav_compass_hdg.data;
		// ned坐标系下的位置、速度
        uav_state.local_x = uav_local_pos.pose.position.y;
        uav_state.local_y = uav_local_pos.pose.position.x;
        uav_state.local_z = - uav_local_pos.pose.position.z;
        uav_state.local_ori_x = uav_local_pos.pose.orientation.x;
        uav_state.local_ori_y = uav_local_pos.pose.orientation.y;
        uav_state.local_ori_z = - uav_local_pos.pose.orientation.z;
        uav_state.local_ori_w = uav_local_pos.pose.orientation.w;
        uav_state.local_lin_x = uav_local_vel.twist.linear.y;
        uav_state.local_lin_y = uav_local_vel.twist.linear.x;
        uav_state.local_lin_z = uav_local_vel.twist.linear.z;
        uav_state.local_ang_x = uav_local_vel.twist.angular.x;
        uav_state.local_ang_y = uav_local_vel.twist.angular.y;
        uav_state.local_ang_z = uav_local_vel.twist.angular.z;
		uav_state.ground_speed = air_ground_speed.groundspeed;
		uav_state.air_speed = air_ground_speed.airspeed;
		uav_state.rel_height = altitude_from_px4.relative;
		uav_state.ned_altitude = altitude_from_px4.local;

		time_t tt = time(NULL);
		tm* t = localtime(&tt);
		int hour = t->tm_hour;
		int minutes = t->tm_min;
		int seconds = t->tm_sec;
		uav_state.hour = hour;
		uav_state.minutes = minutes;
		uav_state.seconds = seconds;

        uav_state_pub.publish(uav_state);

        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}
